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Abstract — We consider the problem of estimating a random 
state vector when there is information about the maximum 
distances between its subvectors. The estimation problem is 
posed in a Bayesian framework in which the minimum mean 
square error (MMSE) estimate of the state is given by the 
conditional mean. Since finding the conditional mean requires 
multidimensional integration, an approximate MMSE estimator 
is proposed. The performance of the proposed estimator is 
evaluated in a positioning problem. Finally, the application 
of the estimator in inequality constrained recursive filtering 
is illustrated by applying the estimator to a dead-reckoning 
problem. The MSE of the estimator is compared with two related 
posterior Cramer-Rao bounds. 



I. Introduction 

Consider the problem of estimating a vector x G K'' with a 
known prior distribution. In the standard Bayesian setup one 
tries to estimate x from a correlated observation y. In this 
letter we deviate from the standard problem and instead seek 
the estimate of x relying on the side information ||xi — Xj || < 
7, where x^ and Xj are subvectors of x, and 7 is given. 

This setup has a range of applications in positioning and lo- 
calization. As an example, consider the problem of estimating 
jointly the positions pi e M'^ and p2 G M'' and the velocities 
^■^ and V2 G M.^ of two points on a human body. In 
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that case, the state is x = [p^ 
there is an upper limit 7 on how far apart body parts can be, 
we have the side information that ||pi — P2II < 7- Similarly, 
consider the problem of estimating the positions of N nodes, 
X = [x.J . . .x^]^, when bounds, ||xi — < jij, on the 
distance between pairs of nodes are given. Further applications 
are possible for sensor fusion of systems subject to non-rigid 
constraints. 

In the literature, there are two main approaches to tackle 
the problem of estimation with nonlinear inequality constraints 
|[T). The first approach uses the side information by passing 
X through a nonlinear function, such that the output always 
fulfills the constraint [2J, L3J. The second approach treats the 
problem probabilistically, and the side information is used to 
form a conditional probability density function (pdf) of x by 
truncating and renormalizing the prior pdf; the support of the 
conditional pdf is a region where the constraint is inactive |j4|, 
||5i . However, when using distance bounds as side information, 
the support of the conditional pdf is infinite. In scenarios where 
the dispersion of the pdf increases without bound, this may 
lead to numerical problems when computing the moments of 
the pdf using e.g., the Monte Carlo methods suggested in [SJ. 

In this letter, we circumvent the problems associated with 
the infinite support pdf, by reformulating the problem via a 
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linear transformation. This also reduces the dimensionality 
of the original problem by a factor two. Based on the new 
problem formulation an efficient method for computing an 
approximate minimum mean square error (MMSE) estimator 
of X given bounds on the distances between its subvectors is 
presented. The method is validated using simulations. 

Notation: A©B denotes the direct sum between matrices A 
and B. A^/^ is the lower-triangular Cholesky factorization of 
a positive definite matrix A. tr{ } denotes the trace operator, 
[A]i is the zth column of A, and A^^ denotes the transpose 
of the inverse matrix A^^, i.e., A^^ = (A^^)^. Further, I„, 
In, and 0„ denote the identity matrix, a vector of ones and 
zeros, respectively. 

II. Problem formulation 

Let the state be defined as x = [x^ xj xj]^ G R'^, 
where xi , X2 G M" are the two subvectors related to the side 
information, Xq G M™ is a subvector holding the auxiliary 
states, and the state dimension d = 2n+m. Further, we assume 
that the prior pdf p{x) of the state is Gaussian with known 
mean rrix and covariance C^, i.e., x ^ J\f{mx, Cx)- 

Now, if we in addition to rrix and are provided with the 
side information c that tells us about the maximum distance 
between the two subvectors, i.e., we have the constraint that 
llLx|| < 7, where L = [l„ — 1„ 0„xm]- Then, we would 
like to compute the MMSE estimate of x given c. The 
estimator and its error covariance matrix are given by the 
mean m^ic and covariance matrix Cx\c of the conditional pdf 
p(x|c) [6|. Since the computation of these moments requires 
multidimensional numerical integration, our aim is to find 
a computationally efficient way to calculate m^|c and C^-i^ 
approximately. The resulting approximations will be denoted 
as X|c and C^. 

III. Proposed solution 

The proposed method to calculate m^ic and C^ic consists 
two steps. In the first step, a state transformation is performed, 
that maps the infinite integration area into a finite area and 
reduces the dimension of the integrals. Thus the transforma- 
tion simplifies the calculation of the conditional mean and 
covariance. In the second step, the integrals are approximated 
using a deterministic sampUng approach. 

A. Variable transformation 

Define a new state vector, given by the invertible linear 
transformation z = Tx G M"^, such that it can be divided 
into the subvectors Zi = xi — X2 G M" and Z2 — [(xi + 
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and z - Af{m^, C^). Here = Tnia; and = TC^^T^. 
In terms of the new state vector, the constraint c can be 
expressed as ||LT^^z|| = ||zi|| < 7, and the constraint thus 
operates only on the sub vector zi. As will been shown next, 
this important property simplifies the calculations of mx\c and 
Cj.|c. Since the means and covariances of the new and original 



State are related via hIt 



T^^m^ and 



we can recast the original problem of calculating mj.|c and 
Cj-ic, into computing m^i^ and C^i^. 

To compute the moments, note that p(z2|zi) is Gaussian 
with mean m^^izj and covariance Cz2\zi given by the affine 
mapping 
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where A = C^^^jC^^^ and u^^ = m^^ — Am^j. 

Next, since zi is given the norm ||zi|| provides no addi- 
tional information and p(z2|zi, ||zi||) is identical to p(z2|zi). 
Similarly, given zi a bound on the norm ||zi|| < 7 yields no 
additional information. Hence p(z2|zi,c) = p(z2|zi) for all 
valid zi, i.e., Vzi G {zi G R" : ||zi|| < 7}. The conditional 
mean mz|c and covariance Cz\c can thus be calculated as 
follows. 

Let the conditional mean be partitioned as 111^1^ = 
[mj 1^ mj 1^]^. The conditional mean of Zi is given by 
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m^iic = / zip(zi|c)dzi, 



(2) 



and the conditional mean of Z2 by 



Z2?j(z2|zi)dz2 

= / (Uz2 + Azi)p(zi|c)dzi 

•J Zi 

= + Am^^ic. 



p{zi\c)dzi 



(3) 



The conditional covariance matrix can be written as 
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and 
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Hence, we need only solve the integrals (|2]i and Q for the 
first and second order moments of p(zi|c). The remaining 
parts of mz|c and Cz|c can be found by a series of affine 
transformations. An important computational advantage of the 
reparameterization, z = Tx, is that the support of p{zi\c) 
is finite unlike the support of p(x|c). Further, a reduction of 
the dimensionality by a factor two compared to the original 
problem is achieved. When n = 1, the integrals (|2]l and ^ 
are given by the mean and variance of a truncated Gaussian 
distribution, which both have closed-form expressions Q. 

B. Approximating the conditional mean and covariance 

When n > 1, no apparent closed-form expressions of 
integrals (|2]l and (|5]l exist. To avoid solving the integrals 
through computationally complex numerical integrations, we 
will approximate them by the convex combinations 
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where the ith sample point z\ and weight Wi, described 
next, are chosen so that the following properties hold true. 
When a fraction a of the probability mass of p{zi) is within 
the boundary of the constraint, the approximated moments 
are unchanged. Otherwise, the sample points adapt to the 
constraint, which ensures that the approximated mean falls 
within its convex boundary and that the dispersion is reduced 
accordingly. Thus, the design parameter a affects when the 
side information becomes effective. 

The proposed deterministic sampling technique is as fol- 
lows. First, as in the sigma-point transformation |8|, 2n + 1 
sample points from p{zi) are generated deterministically by 



where 
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\,\c= h^i^Jp{^i\c)dzi, (5) [mz, -?7y'[C^H^-n i = n + l,.. 
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Here rja is the value that fulfills Pr{77 < rja) ~ a, where 
■q = {zi — mzi)^C7^^(zi — nizi). That is, 77^ is set by the 
confidence ellipse at level a and can be calculated from the 
inverse of the cumulative distribution function (cdf) of ry x^. 

Then, a new set of sample points {z^*^}f"g are generated 
by orthogonally projecting the sample points s'*) that violate 
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i = 0,...,2n. (10) 



the constraint c onto its spherical boundary. That is 

^W^|s« if||sW||<7 

^ llRI^*'''' otherwise 

The constraint-violating points are, in a fashion similar to that 
in |[3], resampled at the boundary. The weights in ([8]) are set 
as 



1 - ^ i = 



, ,2n 



(11) 



This choice yields the properties for ^ described earlierQ 
Once the moments are computed, m^^^ = T^^m^|c and 
Crc\c = T~^C2|cT~^ are obtained using the affine trans- 
formations (O, (|6]l, and (|7]l along with This provides the 
estimator Xj^ and approximated error covariance matrix C^. 

C. Illustration of how the estimator works 

We illustrate the sigma-point approximation and the result- 
ing estimator using the following example. Two objects, with a 
joint spatial Gaussian distribution, are located in M?. The mean 
positions of the objects are m^r^ = O2 and rHx^ = 0.8 • I2 
[m], and the joint state covariance matrix Cx = Cxi ® Cx^, 
where 
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We then provide the side information c with 7 = 1 [m]. 
Setting a — 0.95, the approximated conditional means m.x^\c 
and m.x^\c, along with confidence ellipses, are shown in 
Fig. [U Fig. 12] illustrates the deterministic sampling procedure 
in subsystem zi. The sample points are generated according 
to the confidence ellipse and projected orthogonally when the 
constraint is violated. Observe that the ellipses shrink when 
the side information c becomes available. 

IV. Experimental results 

Using Monte Carlo simulations, the estimator X|c is evalu- 
ated numerically in a positioning and a tracking scenario in R^. 
Throughout the experiments, the side information c is given by 
7 = 1 [m] and the estimator design parameter a = 0.95. As 
a performance measure, the root mean square error (RMSE) 
of the state estimates is used. 

A. Positioning scenario 

In this scenario, the positioning of the two systems Xi and 
X2, is considered. In the Monte Carlo simulation, the positions 
of the two systems were generated by drawing two candidate 
positions from two independent Gaussian distributions, and 
then retaining a realization that fulfilled the constraint with 7. 
The parameters of the pdf were vHx^ = /3l2, Cx^ = (j\l2, 
and m.x2 — O2 and C^s = l2- The empirical RMSE of the 
estimator X|c was calculated using Monte Carlo simulations 
with lO"' runs. Figs. |3] and |4] show the RMSE of the estimator 

'An a close to one yields a confidence ellipse that captures a larger 
probability mass and thus adapts smoother to the constraint. However, setting 
a too large results in sample points with too low weights to approximate the 
truncated pdf. 




Fig. 1. Illustration of how the side information affects the mean positions. 
Shown are the means and approximated conditional means, together with 
the loci corresponding to the 95% confidence ellipses of Gaussian pdfs with 
covariance matrices C^i^ Cx2, Cj-^i^, and C^-^ic, respectively. 
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Fig. 2. Illustration of the sampling technique used to approximate m^^ and 
C^jIc- Sample points before (circles) and after Zj (asterisk) the projection, 
the boundary of the constraint c with 7 = 1 (dashed circle), and the confidence 
ellipses of Czi (dash-dotted) and C^^j^ (solid) are also shown. 



for different values of Ci and 13, respectively. As expected, 
the accuracy improvement of X|c over the prior mean xHx 
increases as the certainty of the position of one object grows, 
or when its prior mean is placed further away. In these cases c 
provides more information. The figures also show y^trjCV}, 
which indicate that the second-order statistics are slightly 
underestimated. 

B. Tracking scenario 

Let us now consider the scenario where we want to fuse 
the position information from two dead-reckoning systems 
mounted on a non-rigid body, but where the body has a 
known finite length. This could, for example, be two foot- 
mounted inertial navigation systems (one system on each 
foot) used to track the position of a person while walk- 
ing inside a building |9|. The joint position state x(fc) = 
[(xi(fc))T (x2(fc))^]^ e M* of the two dead-reckoning 
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Fig. 3. Positioning scenario. RMSE versus ai for / 
c with 7 = 1. 
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Fig. 5. Tracking scenario. RMSE over time for two dead-reckoning systems 
when the side information c is used. The PCRBs for the case with no side 
information and the case with perfect distance information are also shown. 
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Fig. 4. Positioning scenario. RMSE versus /3 for cri 
c with 7 = 1. 
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systems is updated every second via the recursion x(A:) = 
x(A; — 1) + u(fc). The displacements u{k) are measured 
in noise, u(fc) ^ 7\A(u(fc),Q), where Q denotes the error 
covariance of the measured displacements. Due to the integra- 
tive nature of the dead-reckoning recursion, the uncertainty 
of the state x(fc) will grow without bound; the covariance 
of the state uncertainty, P(fc), is given by the recursion 
P(/c) = P{k — 1) + Q, starting from the covariance P(0) 
of the initial position state. 

To reduce the rate at which the position uncertainties grow, 
we can use the side information that the two systems are 
mounted on a non-rigid body of known finite length, i.e., 
we have the side information that ||xi(fc) — X2(fc)|| < 7, Vfc. 
Given the sequence u(fc) = {^{i)}i=Q of measured position 
changes and the general side information /, the conditional 
pdf p(x(fc)|u(fc), /) can be calculated recursively as 



p(x(fc)|u(fc),/) = J p(x(fc)|x(fc - i),a(fc)) 

X p (x(fc - l)|u(fc - 1), /) dx{k - 1), 

starting from the pdf p(xo|/) of the initial position state. The 
transition pdf p(x(fc)|x(fc — l),u(fc)) equals 7V(x(fc — 1) + 
u(fc), Q). The MMSE estimate of x(fc) is given by the mean 
of p(x(fc)|u(fc), /), which is intractable in general. 

When / = c, we use the conditional moments at each time 
instant k and approximate p(x(fc)|u(fc), c) by a Gaussian pdf, 
7V(m3.|c(fc), C^|c(fc)). Then the recursion results in Gaussians 
with computable means that form point estimates X|c(fc). We 
compare this to the case when the side information is the actual 
distances, / = {y(j)}.f^O' where y(fc) = ||xi(fc) — X2(fc)||. 

The posterior Cramer-Rao bound (PCRB) on the RMSE 



fo r / = i2> is ^tr{P(fc)} and for / ^ {y(i)}*^=0' it is 
^ytr{J^^{k)}, where J(fc) is the Fisher information matrix 
of the state (see |10| for details). The performance of the 
estimator is compared with the PCRBs in Fig. |5l where Q = 
10"^ • I4 [m^]. Initially, the RMSE follows the upper PCRB, 
but as the errors of the dead-reckoning systems accumulate, the 
distance bound becomes more informative and the estimator 
tends towards the lower PCRB, which has a lower growth rate 
than the upper PCRB; proof emitted due to space limitations. 

Reproducible research: The proof of convergence to the 
lower PCRB and the Matlab code used in all the simulations 



is available at www.ee.kth.se/~davez/rr-bayes 



V. Conclusion and further work 

We have presented an approximate MMSE estimator that 
uses a given maximum distance between subvectors as side 
information. By reducing the dimensionality of the problem, a 
computationally efficient formulation was given. The estimator 
has a range of potential applications in positioning and local- 
ization. Our further work includes extending the framework to 
larger systems with several distance bounds, and applying it 
to a multi-user indoor navigation system. 
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